/*
 *  td.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos García Orden on 12/07/2010
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "tss.h"

TSS::TSS()
:	Component(),
	lam(),
	gam(),
	s(),
	theta(),
	partial_qdotOld(),
	betac(),
	c(),
	c0(),
	lam0(),
	c10(),
	c11(),
	mu0(),
	mu1(),
	beta0(),
	ppLam(),
	pLam(),
	pGam(),
	pS(),
	fT(),
	Q(),
	h(),
	denom(0.0),
	toldenom(1.0e-6),
	mode()
{}

// ***

TSS::TSS(vector<string>& vp, vector<double>& param, double &ma, int di)
:	Component(vp,di),
	mass(ma),
	lam(),
	gam(0.0),
	theta(300.0),
	partial_qdotOld(0.0),
	betac(4.0),
	c(10.0),
	c0(1.0),	// With 10 oscillates more
	lam0(1),
	c10(100.0),
	c11(0.5),
	mu0(5.0),
	mu1(0.1),
	denom(0.0),
	toldenom(1.0e-6),
	mode("lin"),
	Q0(1.0),
	state(0), // stick (0) or slip (1) 
	QMid(0)
{

	c10 =	param[0];
	c11 =	param[1];
	betac = param[2];
	c0 =	param[3];
	mu0 =	param[4];
	mu1 =	param[5];
	Q0 =	param[6];
	c =	param[7];
	
	pLam = lam;
	pGam = gam;
	pS = s;
	
	if (c10!=0.0) beta0 = 2.0*mu0/c10; 
	else beta0 = 0.0;
		
//	numDofs=2*dim+1;
//	cout << "numDofs = " << numDofs << endl;	
}

TSS::TSS(vector<string>& vp, vector<double>& param, map<string,string>& param2, double &ma, int di)
:	Component(vp,di),
	mass(ma),
	lam(),
	gam(0.0),
	theta(300.0),
	partial_qdotOld(0.0),
	betac(4.0),
	c(10.0),
	c0(1.0),	// With 10 oscillates more
	lam0(1),
	c10(100.0),
	c11(0.5),
	mu0(5.0),
	mu1(0.1),
	denom(0.0),
	toldenom(1.0e-6),
	mode("lin"),
	state(0), // stick (0) or slip (1) 
	Q0(1.0)
{

	c10 =	param[0];
	c11 =	param[1];
	betac = param[2];
	c0 =	param[3];
	mu0 =	param[4];
	mu1 =	param[5];
	Q0 =	param[6];
	c =	param[7];
	
	pLam = lam;
	pGam = gam;
	pS = s;
	ppLam = 0;
	
	if (c10!=0.0) beta0 = 2.0*mu0/c10; 
	else beta0 = 0.0;

	stringstream stnum;

 	if (param2.count("mode")>0)
      	{	
		mode = param2.find("mode")->second;
		//cout << mode << endl;
	}
			
//	numDofs=2*dim+1;
//	cout << "numDofs = " << numDofs << endl;	
}

// ***

void TSS::calcFi(const vector<double>& qddot)
{
	if (mass!=0.0)
	{
		int i;
		int j;
		
		Fi.setZero(numDofs);
		for(i=0; i<numDofsInertia; ++i)
		{
			for(j=0; j<numDofsInertia; ++j)
			{
				if(integratorType==0)  Fi[i] += M(i,j) *qddot[ localGlob[j] ];
				else	Fi[i] += M(i,j) * (dq[j] - dqC[j]);
			}
		}			
	}
}

// ***

// ok
void TSS::calcFq(const int& nt)
{
	
	lam = calcLam(q);
	s = q[numDofs-1];
	
	if (integratorType==0)	// Standard integrator
	{	
		gam = calcGam(pGam,lam,s,deltaT);
	
		fT = partialVLam(lam,gam,s);	// total force
		Q = -partialVGam(lam,gam,s);	// viscous force
		theta = partialVS(lam,gam,s);	// temperature
		
		h = c*(theta - thetab);	// heat flux
	
		for (i=0; i<dim; ++i)
		{
			Fq[i] = (fT/lam)*(q[i]-q[i+dim]);
			Fq[i+dim] = -Fq[i];
		}
		// Fq[numDofs-1] = dq[numDofs-1] - ( (Q*Q)/Eta(theta) - h ) / theta;
/*		
		cout << "lam = "<< lam << endl;
		cout << "gam = "<< gam << endl;
		cout << "s = "<< s << endl;
		cout << "theta = "<< theta << endl;
		cout << "fT = "<< fT << endl;
		cout << "Q = "<< Q << endl;
		cout << "h = "<< h << endl;
*/
//		cout << "Fq[numDofs-1] = " << dq[numDofs-1] << endl;
	}
	else if (integratorType==1 || integratorType==2 ) // Midpoint and consistent
	{		
		if(qMid.size()==0)
		{
			qMid.resize(q.size());
			dqMid.resize(dq.size());
		}
		
		for(i=0; i<qMid.size();i++)
		{
			qMid[i]=0.5*(q[i]+qC[i]);
			dqMid[i]=0.5*(dq[i]+dqC[i]);
		}
		
		lamMid = calcLam(qMid);				
		if (integratorType==1)
		{
			sMid = qMid[numDofs-1];	// Midpoint

			gam = calcGam(pGam, lamMid, sMid, deltaT);
		
			gamMid = 0.5*(gam + pGam);

			fTMid = partialVLam(lamMid,gamMid,sMid);	// total force
			QMid = -partialVGam(lamMid,gamMid,sMid);	// viscous force
			thetaMid = partialVS(lamMid,gamMid,sMid);	// temperature
		}
		else	// Consistent
		{

			// Stick

			lamMid = calcLam(qMid);				
			//cout << "state is " << state << endl;			
			// gam = calcGam(pGam, lam, s, deltaT);
			if (state == 0) // stick
			{ 
				gam = pGam;
			
				fTMid = partialVLam(lam,gam,s);	// total force
				QMid = -partialVGam(lam,gam,s);	// viscous force
				thetaMid = partialVS(lam,gam,s);// temperature	
			}
			else { 		// slip
				
				gam = calcGam(pGam, lam, s, deltaT);
				
				fTMid = partialVLam(lam,gam,s);	// total force
				QMid = -partialVGam(lam,gam,s);	// viscous force
				thetaMid = partialVS(lam,gam,s);// temperature
			}
		
		}
		
		hMid = c*(thetaMid - thetab);	// heat flux
		//cout << fTMid << "\t"	<< QMid << "\t" << thetaMid << "\t" << endl;	
		for (i=0; i<dim; ++i)
		{
			Fq[i] = (fTMid/lamMid)*(qMid[i]-qMid[i+dim]);
			Fq[i+dim] = -Fq[i];
		}

		if (state == 0)	//stick
		{
			Fq[numDofs-1] = dqMid[numDofs-1] + hMid/thetaMid;
		}
		else //slip
		{
			double coef = 1; 
			Fq[numDofs-1] = dqMid[numDofs-1] - ( coef*QMid*(gam-pGam)/deltaT - hMid )/thetaMid;
		}
		
		
		// Fq[numDofs-1] = dqMid[numDofs-1] - ( (QMid*QMid)/Eta(thetaMid) - hMid ) / thetaMid;
		//cout << QMid << "\t" << gam << "\t" << pGam << "\t" << nt << "\t" << thetaMid << "\t" << dqMid[numDofs-1] << "\t" << hMid << endl;
		// Fq[numDofs-1] = dqMid[numDofs-1] - ( QMid*abs((gam-pGam)/0.05) - hMid ) / thetaMid;
		//Fq[numDofs-1] = dqMid[numDofs-1] - ( (QMid*QMid)/Eta(thetaMid) - hMid ) / thetaMid;
		//Fq[numDofs-1] = dqMid[numDofs-1] - ( (QMid*QMid)/Eta(thetaMid) - hMid ) / thetaMid;
		/*for(int ii=0; ii < Fq.size(); ++ii)
		{
			cout << "fq " << Fq[ii] << endl;
		}*/
	}
/*
	for (i=0; i<numDofs; ++i)
	{
		cout << "Fq[" << i << "] = " << Fq[i] << endl;
	}
*/
}

// ***

void TSS::calcFt(const double& t, const double& grav, const int& gravDir)
{}

// ***

void TSS::calcKi(double & partial_qddot)
{
	if (mass!=0.0)
	{
		if(partial_qddot!=partial_qddotOld)	// In order to compute Ki just once
		{
//			int i;
//			int j;
//			for(i=0; i<numDofs; i++)
//				for(j=0; j<numDofs; j++) Ki[i][j] = M(i,j)*partial_qddot;
			Ki.block(0,0,numDofsInertia,numDofsInertia) = M * partial_qddot;
		}	
		partial_qddotOld = partial_qddot;		
	}
}

// ***

void TSS::calcKq(int nt)
{
	calcLocalKqNumeric();	// Analytical tangent, TO BE DONE
}

// ***

void TSS::calcKdq(double & partial_qdot)
{
	if (integratorType!=0) partial_qdot/= 2.0;	// Midpoint and consistent
	if(partial_qdot!=partial_qdotOld)	// In order to compute Kdq just once
	{
		Kdq(numDofs-1,numDofs-1) = partial_qdot;
	}
	partial_qdotOld = partial_qdot;
}

// ***

void TSS::calcM(const vector<double>& q, const double& t)
{
	if (mass!=0.0)
	{
		int i;
		if(M.size()==0)
		{	
			M.setZero(numDofsInertia,numDofsInertia);
		}
		// Consistent mass matrix
		if(numDofsInertia==6)	// dim=3
		{
			M(0,0) = mass/3.;
			M(0,3) = mass/6.; 
			M(1,1) = mass/3.; 
			M(1,4) = mass/6.;
			M(2,2) = mass/3.;
			M(2,5) = mass/6.;
			M(3,0) = M(0,3);
			M(3,3) = mass/3.;
			M(4,1) = M(1,4); 
			M(4,4) = mass/3.;
			M(5,2) = M(2,5);
			M(5,5) = mass/3.;	
		} 
		else if(numDofsInertia==4)	// dim=2
		{
			M(0,0) = mass/3.;
			M(0,2) = mass/6.;
			M(1,1) = mass/3.;
			M(1,3) = mass/6.;
			M(2,0) = M(0,2);
			M(2,2) = mass/3.;
			M(3,1) = M(1,3);
			M(3,3) = mass/3.;
		} 
		else			// dim=1
		{
			M(0,0) = mass/3.;
			M(0,1) = mass/6.;
			M(1,0) = M(0,1);
			M(1,1) = mass/3.;
		}			
	}
}

// ***

void TSS::calcMLumped(const vector<double>& q, const double& t)
{
	if (mass!=0.0)
	{
		if (MLumped.size()==0)
		{
			MLumped.resize(numDofsInertia);
		}
		
		int i;
		for (i=0; i<numDofsInertia; ++i)	MLumped[i] = mass/2.0;
	}
}

// ***

void TSS::setDofs(void)
{
	map<string,Point*,less<string> >::iterator it;
	for(it=mPoints.begin(); it!=mPoints.end(); it++)
	{
		for(int j=0; j<dim; j++)
		{
			(*it).second->setDof(0,j);
			(*it).second->setDofInertia(0,j);
		}
	}
	// No inertia dofs added
	
	// It must not add any dof, but must set variable numDofs
	numDofs=2*dim+1;
	numDofsInertia = 2*dim;

	dofT.set(0,1);	// Component dof added at position 0
}

// ***

void TSS::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{	
	/*
	cout << "numDofs = " << numDofs << endl;
	cout << "localGlob is " << endl;
	printSTLVector(localGlob);
	*/
	int p;

	q.resize(numDofs);
	
	for(p=0; p<pointNames.size(); ++p)
	{		
		for(j=0; j<dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
			q_in[ localGlob[p*dim+j] ] = mPoints[pointNames[p]]->r[j];
		}
	}
	auxd = 0.0;
	for (i=0; i<dim; ++i)
	{
		auxd+= (q_in[ localGlob[i] ]-q_in[ localGlob[i+dim] ])*(q_in[ localGlob[i] ]-q_in[ localGlob[i+dim] ]);
	}
	lam = sqrt(auxd);
	lam0 = lam;
	s = entropy(lam, gam, theta);
	
	q_in[ localGlob[numDofs-1] ] = s;		// Default, initial entropy
/*	
	for(i=0; i<q_in.size(); ++i)	cout << " " << q_in[i];
	cout << endl;
*/	
	pLam = lam;
	pGam = gam;
	pS = s;	
/*
	cout << "initial lam = " << lam << endl;
	cout << "initial gam = " << gam << endl;
	cout << "initial theta = " << theta << endl;
	cout << "initial s = " << s << endl;
*/

	// Initial velocity
	// Default: initial velocity dofs are null
	dq.resize(numDofs);
}

// ***

void TSS::setMatAtComp(Material* ma)
{}

// ***

void TSS::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augLagFlag)
{
	Fq.setZero(numDofs);
	if(grav!=0.0)
	{
		int i;
		int j;		
		Ft.resize(numDofs);
		// Build Ft once for all  Ft= mass * g
		Ft[gravDir-1] = - mass * grav / 2.0;
		Ft[dim+gravDir-1] = - mass * grav / 2.0;
	}
	
	if(st!=0 && mass!=0.0)
	{
		Fi.resize(numDofs);
		//M.setZero(numDofsInertia,numDofsInertia);
	}	
}

// ***

//void TSS::initializeCompTang(const bool& st, const bool& pf)
void TSS::initializeCompTang(const bool& st)
{
	Kq.setZero(numDofs,numDofs);

	if(st!=0)
	{
		if (mass!=0.0)
		{
//			Ki.resize(numDofs);
//			for(i=0; i<numDofs;++i)	Ki[i].resize(numDofs);
			Ki.setZero(numDofs,numDofs);
		}
//		Kdq.resize(numDofs);
//		for(i=0; i<numDofs;++i)	Kdq[i].resize(numDofs);
		Kdq.setZero(numDofs,numDofs);
	}
}

// ***

void TSS::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	// Kinetic
	if(dq.size()!=0 && (M.size()!=0 || MLumped.size()!=0) )
	{
		int j;
		for (i=0; i<numDofs; ++i)	dq[i] = dq_in[localGlob[i]];
		for (i=0; i<numDofsInertia; ++i)
		{
			if (M.size()!=0)	// Consistent
			{
				for (j=0; j<numDofsInertia; ++j)
				{
					Ekin+= 0.5 * dq[i] * M(i,j) * dq[j];
				}
			}
			else	// Lumped
			{
				Ekin+= 0.5 * dq[i] * MLumped[i] * dq[i];
			}						
		}
	}
	
	Epot+= V(lam,gam,s);

	if (integratorType==0)	// Trapezoidal.  Other integrators TO BE DONE
	{
		Eamb+= c*( 0.5*(partialVS(lam,gam,s)+partialVS(pLam, pGam, pS)) - thetab)*deltaT;	// heat flux
	}
	else if (integratorType==1)	// Midpoint
	{
		Eamb+= c*( partialVS(lamMid, gamMid, sMid) - thetab)*deltaT;
	}
	else if (integratorType==2)	// Consistent
	{
		Eamb+= c*( partialVS(lam, gam, s) - thetab)*deltaT;
	}
}

// ***

void TSS::calcEntropy(double& Entropy, double& EntropyAmb)
{
	Entropy+= s;

	if (integratorType==0)	// Trapezoidal.  Other integrators TO BE DONE
	{
		EntropyAmb+= c*( 0.5*(partialVS(lam,gam,s)+partialVS(pLam, pGam, pS)) - thetab )*deltaT/thetab;
	}
	else if (integratorType==1)	// Midpoint
	{
		EntropyAmb+= c*( partialVS(lamMid,gamMid,sMid)- thetab )*deltaT/thetab;
	}
	else if (integratorType==2)	// Consistent
	{
		EntropyAmb+= c*( partialVS(lam,gam,s)- thetab )*deltaT/thetab;
	}	
}

// ***

void TSS::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{}	

// ***

double TSS::V(double lam, double gam, double s)
{
	
	if (!mode.compare("log"))
	{
		t5 = log(lam / lam0);
		t6 = t5*t5;
		t7 = c11 * t6;
		t9 = betac * t5;
		t12 = gam*gam;
		t16 = beta0 * gam;
		t22 = beta0 * lam;
		t29 = 0.1e1 / lam;
		t35 = exp(-(-0.2e1 * s * lam + t7 * lam + 0.2e1 * t9 * lam + 0.2e1 * mu1 * t12 * lam - 0.2e1 * t16 * c11 * t5 - 0.2e1 * t16 * betac + t22 * t7 + 0.2e1 * t22 * t9) / c0 * t29 / (0.1e1 + beta0) / 0.2e1);
		t36 = t35 * thetab;
		t37 = t36 - thetab;
		t39 = c10 - c11 * t37;
		t41 = t39 * t6 / 0.2e1;
		t42 = betac * t37;
		t43 = t42 * t5;
		t44 = log(t35);
		t47 = c0 * (t36 - thetab - t36 * t44);
	
	return t41 - t43 + t47 + (mu0 - mu1 * t37) * t12 - beta0 * (t39 * t5 * t29 - t42 * t29) * gam + beta0 * (t41 - t43 + t47) + t36 * s;
	}

	if (!mode.compare("lin"))
	{
		t2 = (lam *lam);
		t4 = (c11 * lam);
		t7 = (lam0 *lam0);
		t13 = (beta0 * gam);
		t16 = (gam *gam);
		t24 = (beta0 * betac);
		t27 = (beta0 * c11);
		t35 = 2 * s - c11 * t2 + 2 * t4 * lam0 - c11 * t7 - 2 * betac * lam + 2 * betac * lam0 + 2 * t13 * betac - 2 * mu1 * t16 + 2 * t13 * t4 - 2 * t13 * c11 * lam0 - 2 * t24 * lam - t27 * t2 + 2 * t27 * lam * lam0 - t27 * t7 + 2 * t24 * lam0;
		t42 = exp((t35 / c0 / (1 + beta0)) / 0.2e1);
		t43 = t42 * thetab;
		t44 = t43 - thetab;
		t46 = c10 - c11 * t44;
		t47 = lam - lam0;
		t48 = t47 *t47;
		t50 = t46 * t48 / 0.2e1;
		t51 = betac * t44;
		t52 = t51 * t47;
		t53 = log(t42);
		t56 = c0 * (t43 - thetab - t43 * t53);
		return t67 = t50 - t52 + t56 + (mu0 - mu1 * t44) * t16 - beta0 * (t46 * t47 - t51) * gam + beta0 * (t50 - t52 + t56) + t43 * s;
	}

	
}

// ***

double TSS::dlamV(double lam, double gam, double s)
{
	if (!mode.compare("log"))
	{
		t4 = log(lam / lam0);
		t5 = c11 * t4;
		t7 = t4 * t4;
		t8 = c11 * t7;
		t10 = betac * t4;
		t12 = gam * gam;
		t13 = (mu1 * t12);
		t15 = beta0 * gam;
		t16 = 0.1e1 / lam;
		t20 = beta0 * c11;
		t24 = beta0 * betac;
		t28 = -(2 * s) + 0.2e1 * t5 + t8 + 0.2e1 * betac + 0.2e1 * t10 + (2 * t13) - 0.2e1 * t15 * c11 * t16 + t20 * t7 + 0.2e1 * t20 * t4 + 0.2e1 * t24 * t4 + 0.2e1 * t24;
		t29 = 0.1e1 / c0;
		t32 = 0.1e1 / (0.1e1 + beta0);
		t33 = t16 * t32;
		t46 = beta0 * lam;
		t51 = (-0.2e1 * s * lam + t8 * lam + 0.2e1 * t10 * lam + 0.2e1 * t13 * lam - 0.2e1 * t15 * t5 - 0.2e1 * t15 * betac + t46 * t8 + 0.2e1 * t46 * t10) * t29;
		t52 = lam * lam;
		t53 = 0.1e1 / t52;
		t56 = -t28 * t29 * t33 + t51 * t53 * t32;
		t57 = c11 * t56 / 0.2e1;
		t60 = exp(-t51 * t33 / 0.2e1);
		t61 = t60 * thetab;
		t64 = t57 * t61 * t7 / 0.2e1;
		t65 = t61 - thetab;
		t67 = c10 - c11 * t65;
		t68 = t67 * t4;
		t69 = t68 * t16;
		t70 = betac * t56 / 0.2e1;
		t72 = t70 * t61 * t4;
		t73 = betac * t65;
		t74 = t73 * t16;
		t76 = log(t60);
		t78 = c0 * t56 * t61 * t76 / 0.2e1;
	
		return -t64 + t69 - t72 - t74 - t78 - mu1 * t56 * t61 * t12 / 0.2e1 - beta0 * (-t57 * t60 * thetab * t4 * t16 + t67 * t53 - t68 * t53 - t70 * t61 * t16 + t73 * t53) * gam + beta0 * (-t64 + t69 - t72 - t74 - t78) + t56 * t60 * thetab * s / 0.2e1;
	}

	if (!mode.compare("lin"))
	{
		t1 = (c11 * lam);
		t2 = (c11 * lam0);
		t3 = (beta0 * gam);
		t5 = (beta0 * betac);
		t6 = (beta0 * c11);
		t9 = (-t1 + t2 - betac + t3 * c11 - t5 - t6 * lam + t6 * lam0);
		t11 = 1 / c0;
		t12 = 2 * c11 * t9 * t11;
		t14 = 1 / (1 + beta0);
		t16 = (lam * lam);
		t20 = (lam0 *lam);
		t28 = (gam *gam);
		t44 = 2 * s - c11 * t16 + 2 * t1 * lam0 - c11 * t20 - 2 * betac * lam + 2 * betac * lam0 + 2 * t3 * betac - 2 * mu1 * t28 + 2 * t3 * t1 - 2 * t3 * t2 - 2 * t5 * lam - t6 * t16 + 2 * t6 * lam * lam0 - t6 * t20 + 2 * t5 * lam0;
		t48 = exp((t44 * t11 * t14) / 0.2e1);
		t49 = t14 * t48;
		t50 = lam - lam0;
		t51 = t50 * t50;
		t55 = t12 * t49 * thetab * t51 / 0.4e1;
		t56 = t48 * thetab;
		t57 = t56 - thetab;
		t58 = c11 * t57;
		t60 = (c10 - t58) * t50;
		t62 = 2 * betac * t9 * t11;
		t64 = t49 * thetab * t50;
		t66 = t62 * t64 / 0.2e1;
		t67 = betac * t57;
		t69 = log(t48);
		t72 = t9 * t14 * t56 * t69;
		return t94 = -t55 + t60 - t66 - t67 - t72 - mu1 * t9 * t11 * t49 * thetab * t28 - beta0 * (-t12 * t64 / 0.2e1 + c10 - t58 - t62 * t49 * thetab / 0.2e1) * gam + beta0 * (-t55 + t60 - t66 - t67 - t72) + t9 * t11 * t14 * t56 * s;
	}
}

// ***

double TSS::dgamV(double lam, double gam, double s)
{
	if (!mode.compare("log"))
	{
		t7 = log(lam / lam0);
		t12 = 0.4e1 * mu1 * gam * lam - 0.2e1 * beta0 * c11 * t7 - 0.2e1 * beta0 * betac;
		t13 = c11 * t12;
		t14 = 0.1e1 / c0;
		t15 = 0.1e1 / lam;
		t16 = t14 * t15;
		t19 = 0.1e1 / (0.1e1 + beta0);
		t22 = t7 * t7;
		t23 = c11 * t22;
		t25 = betac * t7;
		t28 = gam * gam;
		t32 = beta0 * gam;
		t38 = beta0 * lam;
		t47 = exp(-(-0.2e1 * s * lam + t23 * lam + 0.2e1 * t25 * lam + 0.2e1 * mu1 * t28 * lam - 0.2e1 * t32 * c11 * t7 - 0.2e1 * t32 * betac + t38 * t23 + 0.2e1 * t38 * t25) * t14 * t15 * t19 / 0.2e1);
		t48 = t19 * t47;
		t52 = t13 * t16 * t48 * thetab * t22 / 0.4e1;
		t53 = betac * t12;
		t56 = t48 * thetab * t7;
		t58 = t53 * t16 * t56 / 0.2e1;
		t61 = t47 * thetab;
		t62 = log(t47);
		t65 = t12 * t15 * t19 * t61 * t62 / 0.2e1;
		t72 = t61 - thetab;
		t77 = lam * lam;
		t78 = 0.1e1 / t77;
	
		return t52 + t58 + t65 + mu1 * t12 * t16 * t48 * thetab * t28 / 0.2e1 + 0.2e1 * (mu0 - mu1 * t72) * gam - beta0 * (t13 * t14 * t78 * t56 + t53 * t14 * t78 * t19 * t61) * gam / 0.2e1 - beta0 * ((c10 - c11 * t72) * t7 * t15 - betac * t72 * t15) + beta0 * (t52 + t58 + t65) - t12 * t14 * t15 * t48 * thetab * s / 0.2e1;
	}

	if (!mode.compare("lin"))
	{
		t1 = (beta0 * betac);
		t5 = (beta0 * c11);
		t10 = 2 * t1 - 4 * mu1 * gam + 2 * t5 * lam - 2 * t5 * lam0;
		t12 = 1 / c0;
		t13 = c11 * t10 * t12;
		t15 = 1 / (1 + beta0);
		t17 = lam *lam;
		t19 = c11 * lam;
		t22 = lam0 *lam0;
		t28 = beta0 * gam;
		t31 = gam * gam;
		t48 = 2 * s - c11 * t17 + 2 * t19 * lam0 - c11 * t22 - 2 * betac * lam + 2 * betac * lam0 + 2 * t28 * betac - 2 * mu1 * t31 + 2 * t28 * t19 - 2 * t28 * c11 * lam0 - 2 * t1 * lam - t5 * t17 + 2 * t5 * lam * lam0 - t5 * t22 + 2 * t1 * lam0;
		t52 = exp((t48 * t12 * t15) / 0.2e1);
		t53 = t15 * t52;
		t54 = lam - lam0;
		t55 = t54 * t54;
		t59 = t13 * t53 * thetab * t55 / 0.4e1;
		t61 = betac * t10 * t12;
		t63 = t53 * thetab * t54;
		t65 = t61 * t63 / 0.2e1;
		t67 = t52 * thetab;
		t68 = log(t52);
		t71 = t10 * t15 * t67 * t68 / 0.2e1;
		t78 = t67 - thetab;
		return t102 = -t59 - t65 - t71 - mu1 * t10 * t12 * t53 * thetab * t31 / 0.2e1 + 0.2e1 * (mu0 - mu1 * t78) * gam - beta0 * (-t13 * t63 - t61 * t53 * thetab) * gam / 0.2e1 - beta0 * ((c10 - c11 * t78) * t54 - betac * t78) + beta0 * (-t59 - t65 - t71) + t10 * t12 * t15 * t67 * s / 0.2e1;
	}
}

// ***

double TSS::entropy(double lam, double gam, double theta)
{
	if (!mode.compare("log"))
	{
		t3 = log(lam / lam0);
		t4 = t3 * t3;
		t6 = c11 * t4 / 0.2e1;
		t7 = betac * t3;
		t10 = log(theta / thetab);
		t11 = c0 * t10;
		t12 = gam * gam;
		t15 = 0.1e1 / lam;
	
		return t6 + t7 + t11 + mu1 * t12 + beta0 * (-c11 * t3 * t15 - betac * t15) * gam - beta0 * (-t6 - t7 - t11);	
	}

	if (!mode.compare("lin"))
	{
		t1 = lam - lam0;
		t2 = t1 * t1;
		t4 = c11 * t2 / 0.2e1;
		t5 = betac * t1;
		t8 = log(theta / thetab);
		t9 = c0 * t8;
		t10 = gam *gam;
		return t18 = t4 + t5 + t9 + mu1 * t10 + beta0 * (-c11 * t1 - betac) * gam - beta0 * (-t4 - t5 - t9);
	}
}

// ***

double TSS::dentV(double lam, double gam, double s)
{
	if (!mode.compare("log"))
	{
		t5 = log(lam / lam0);
		t6 = t5 * t5;
		t7 = c11 * t6;
		t9 = betac * t5;
		t12 = gam * gam;
		t16 = beta0 * gam;
		t22 = beta0 * lam;
		t35 = exp(-(-0.2e1 * s * lam + t7 * lam + 0.2e1 * t9 * lam + 0.2e1 * mu1 * t12 * lam - 0.2e1 * t16 * c11 * t5 - 0.2e1 * t16 * betac + t22 * t7 + 0.2e1 * t22 * t9) / c0 / lam / (0.1e1 + beta0) / 0.2e1);
		return t35 * thetab;
	}

	if (!mode.compare("lin"))
	{
		t2 = (lam * lam);
		t4 = (c11 * lam);
		t7 = (lam0 * lam0);
		t13 = (beta0 * gam);
		t16 = (gam * gam);
		t24 = (beta0 * betac);
		t27 = (beta0 * c11);
		t35 = 2 * s - c11 * t2 + 2 * t4 * lam0 - c11 * t7 - 2 * betac * lam + 2 * betac * lam0 + 2 * t13 * betac - 2 * mu1 * t16 + 2 * t13 * t4 - 2 * t13 * c11 * lam0 - 2 * t24 * lam - t27 * t2 + 2 * t27 * lam * lam0 - t27 * t7 + 2 * t24 * lam0;
		t42 = exp((t35 / c0 / (1 + beta0)) / 0.2e1);
		return t43 = t42 * thetab;
	}
}

// ***

double TSS::integrateGam1ex(double& inigam, double& Lam, double& S, double& dt)
{
	// Explicit Euler
	// return inigam - dt*partialVGam(pLam, pGam, pS)/eta(pLam,pGam,pS);
}

// ***

double TSS::integrateGam1im(double& inigam, double& Lam, double& S, double& dt)
{
/*	// Implicit Euler
	double res;
	double resM;
	double resP;
	double tolres = 1.0e-6;
	double kGam;
//	double perturbation = 1.0e-7;

	gam = inigam;
	res = 1.0;
		
	while (fabs(res)>tolres)
	{
		res = gam - pGam + dt*partialVGam(Lam, gam, S)/eta(Lam,gam,S);
		resM = (gam-perturbation) - pGam + dt*partialVGam(Lam, gam-perturbation, S)/eta(Lam,gam-perturbation,S);
		resP = (gam+perturbation) - pGam + dt*partialVGam(Lam, gam+perturbation, S)/eta(Lam,gam+perturbation,S);
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;
	}
	return gam;
*/
}

// ***

double TSS::integrateGamMid(double& inigam, double& Lam, double& S, double& dt)
{
	double res;
	double resM;
	double resP;
	double tolres = 1.0e-8;
	double kGam;
	//double temp, tempM, tempP;
	double coef;

	gam = inigam;
	res = 2.0*tolres;	

	// Midpoint

	coef = copysign( 1, (pLam - ppLam) );

	//cout << coef << endl;
	//cout << "lam " << lam << " gam " << inigam << " S " << S << endl; 
	double gamMid;		
	while (fabs(res)>tolres)
	{
	
		if (integratorType!=2) gamMid = 0.5*(gam+pGam);
		else gamMid = gam;
		
		res = (-partialVGam(Lam, gamMid, S)-coef*Q0);
		
		resM =   (-partialVGam(Lam, gamMid-perturbation, S)-coef*Q0);
		resP =   (-partialVGam(Lam, gamMid+perturbation, S)-coef*Q0);
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;

		

	}
	
	return gam;
}

// ***
/*
double TSS::integrateGamCon(double& inigam, double& Lam, double& S, double& dt)
{
	double res;
	double resM;
	double resP;
	double tolres = 1.0e-6;
	double kGam;
	double temp, tempM, tempP;
	
	gam = inigam;
	res = 2.0*tolres;	
	
	// Consistent
	
	while (fabs(res)>tolres)
	{
		temp = partialVS(Lam,gam,S);
		res = gam - pGam + dt*partialVGam(Lam, gam, S)/Eta(temp);
		tempM = partialVS(Lam,gam-perturbation,S);
		tempP = partialVS(Lam,gam+perturbation,S);
		resM = (gam-perturbation) - pGam + dt*partialVGam(Lam, gam-perturbation, S)/Eta(tempM);
		resP = (gam+perturbation) - pGam + dt*partialVGam(Lam, gam+perturbation, S)/Eta(tempP);
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;
	}
	
	return gam;
}
*/
// ***

double TSS::integrateGamTra(double& inigam, double& Lam, double& S, double& dt)
{
/*	double res;
	double resM;
	double resP;
	double tolres = 1.0e-6;
	double kGam;
	
	gam = inigam;
	res = 2.0*tolres;	
	
	// Trapezoidal rule
	
	while (fabs(res)>tolres)
	{
		res = gam - pGam + dt*0.5*( partialVGam(pLam, pGam, pS)/eta(pLam,pGam,pS) + partialVGam(Lam, gam, S)/eta(Lam,gam,S) );
		resM = (gam-perturbation) - pGam + dt*0.5*( partialVGam(pLam, pGam, pS)/eta(pLam,pGam,pS) + partialVGam(Lam, gam-perturbation, S)/eta(Lam,gam-perturbation,S) );
		resP = (gam+perturbation) - pGam + dt*0.5*( partialVGam(pLam, pGam, pS)/eta(pLam,pGam,pS) + partialVGam(Lam, gam+perturbation, S)/eta(Lam,gam+perturbation,S) );
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;
	}
	
	return gam;*/
}

// ***

void TSS::updateComp(void)
{
	ppLam = pLam;
	pLam = lam;
	pGam = gam;
	pS = s;

	state = 0;
/*
	cout << "Pillado " << endl;
	cout << "pLam = " << pLam << endl;
	cout << "pGam = " << pGam << endl;
	cout << "pS = " << pS << endl;
*/
}

// ***

double TSS::calcLam(vector<double>& v)
{
	auxd = 0.0;
	for (i=0; i<dim; ++i)
	{
		auxd+= (v[i]-v[i+dim])*(v[i]-v[i+dim]);
	}
	return sqrt(auxd);	
}

// ***

void TSS::setInitialConditions(vector<double>& q_in, vector<double>& dq_in)
{
	// Initial condition in temperature must be transformed in entropy
	vector<double> pos(2*dim);
	int i;
	double auxLam;
	double auxTheta;
	double auxS;
	double auxQ;
	double auxH;

	for (i=0; i<2*dim; ++i)	pos[i] = q_in[localGlob[i]];
	auxLam = calcLam(pos);
	auxS = entropy(auxLam, 0.0, theta);
	q_in[ localGlob[numDofs-1] ] = auxS;
	
	// Initial entropy velocity is computed from the initial conditions
	auxQ = -partialVGam(auxLam,0.0,auxS);	// viscous force
	auxH = c*(theta - thetab);	// heat flux
	dq_in[ localGlob[numDofs-1] ] = auxH  / theta;

/*
	cout << "lamini = " << auxLam << endl;
	cout << "theta = " << theta << endl;
	cout << "sini = " << auxS << endl;
	cout << "dsini = " << dq_in[ localGlob[numDofs-1] ] << endl;
	cout << "lcg " << localGlob[numDofs-1] << endl; 
*/
}

// ***

double TSS::getTemperature(void)
{
	return dentV(lam,gam,s);
}

// ***

double TSS::calcGam(double& gamini, double& Lam, double& S, double& dt)
{
	if (integratorType==0)
	{
		return integrateGamTra(gamini, Lam, S, dt);	// Trapezoidal.  Other integrators TO BE DONE
	}
	else if (integratorType==1 || integratorType==2)
	{
		return integrateGamMid(gamini, Lam, S, dt);	// Midpoint	and consistent
	}
	
//	return integrateGam1ex(gamini,Lam,S,dt);	// Explicit Euler
//	return integrateGam1im(gamini,Lam,S,dt);	// Implicit Euler
}

// ***

void TSS::setIniDofs(vector<double>& k)
{
	theta = k[0];
}

// ***

void TSS::calcLocalKqNumeric()
{
	int j;
 
	if(Fq.size()!=0)
	{
		if(Kq.size()==0)
		{
		  Kq.setZero(numDofs,numDofs);
		}
 
		for (j=0;j<numDofs; ++j)
		{
			q[j]+= perturbation;
			calcFq(1);      // 1 means no simultaneous tangent computation (if available)
 
		        Kq.col(j) = Fq;
 
			q[j]-= 2.0*perturbation;
			calcFq(1);      // 1 means no simultaneous tangent computation (if available)
 
			Kq.col(j) -= Fq;      
 
			q[j]+= perturbation;
		}
		Kq /= (2.0*perturbation);
		calcFq(1);      // 1 means no simultaneous tangent computation (if available)
 
 // cout << "Kq es " << endl;
 // printSTLMatrix(Kq);
 
	}       
 }  
// ***

double TSS::DlamV(double lam, double gam, double s)
{
	denom = 2.0*(lam-pLam);
//	if (denom!=0.0)
	if ( fabs(denom)>toldenom )
	{
		return ( V(lam,gam,s)-V(pLam,gam,s)+V(lam,pGam,pS)-V(pLam,pGam,pS) )/denom;
	}
	else 
	{
		return ( dlamV(pLam,gam,s) + dlamV(pLam,pGam,pS) )/2.0;
	}

	
}	

// ***

double TSS::DgamV(double lam, double gam, double s)
{

		
	denom = 2.0*(gam-pGam);
	//cout << "in DgamV: pGam " << pGam << " denom " << denom << endl;
//	if (denom!=0.0)
	if ( fabs(denom)>toldenom)
	{
		return ( V(pLam,gam,s)-V(pLam,pGam,s)+V(lam,gam,pS)-V(lam,pGam,pS) )/denom;
	}
	else
	{
		return ( dgamV(pLam,pGam,s) + dgamV(lam,pGam,pS) )/2.0;
	}
}	

// ***

double TSS::DentV(double lam, double gam, double s)
{
	denom = 2.0*(s-pS);
//	if (denom!=0.0)
	if ( fabs(denom)>toldenom)
	{
		return ( V(pLam,pGam,s)-V(pLam,pGam,pS)+V(lam,gam,s)-V(lam,gam,pS) )/denom;	
	}
	else
	{
		return ( dentV(pLam,pGam,pS) + dentV(lam,gam,pS) )/2.0;
	}
}	

// ***

double TSS::partialVLam(double lam, double gam, double s)
{
	if (integratorType!=2)	return dlamV(lam, gam, s);	// Standard gradient
	else return DlamV(lam, gam, s);						// Discrete gradient
}

// ***

double TSS::partialVGam(double lam, double gam, double s)
{
	if (integratorType!=2)	return dgamV(lam, gam, s);	// Standard gradient
	else return DgamV(lam, gam, s);						// Discrete gradient
}

// ***

double TSS::partialVS(double lam, double gam, double s)
{
	if (integratorType!=2)	return dentV(lam, gam, s);	// Standard gradient
	else return DentV(lam, gam, s);						// Discrete gradient
}

// ***

void TSS::calcPhi(const vector<double>& q)
{}

// ***

void TSS::calcDPhi(const vector<double>& q)
{}

// ***

void TSS::calcMSmooth(const vector<double>& q_in, const double& t)
{
}

// ***

void TSS::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{
	// setEta();

	if(FSmooth.size() == 0)
	{
		FSmooth.setZero(1,1);
//		FSmooth.resize(1);
//		FSmooth[0].resize(1);
	}

		
	  ofstream myfile;
	  myfile.open ("force.txt",ios::app);
	  myfile << Fq[3] << "\t" << Q0 << "\t" << QMid << "\t" << fTMid <<  "\t" << gam <<endl; 
	  myfile.close();
		
	  //cout << "zarasv " << Q0 <<endl;
	

}

int TSS::checkState()
{

	// cout << QMid << "\t" << Q0 << endl;
	if (fabs(QMid) > Q0) 
	{
		return state = 1; // slip

		
	}
	else 
	{
		return state = 0; // stick
	}
}
